//conrot_servo.cpp
//Erik Wright
/*
This cpp file provides the implementation of the inherited class
conrot_servo.  It is of private inheritance with the pwm_servo class.
Thus it is able to provide a linear linked list of servo motion events.
The main utility of the inheritance is for calculating the current and future displacement and velocity.

*/

#include "conrot_servo.h"
#include <iostream>
#include <cstdio>

using namespace std;




//------------------  Public Functions  -----------------------------

//~conrot_servo
/*
This function is the class destructor.  It is meant to provide
deallocation of any dynamic memory created.
INPUT:	none
OUTPUT:	memory reallocation
*/
conrot_servo::~conrot_servo()
{
	//no dynamic memory
	//inherited class destructor is called after
}



//conrot_servo()
/*
This function is the default class constructor.  It initializes the 
parent class as a list with no nodes.  The list will need to be created.
INPUT:	none
OUTPUT:	none
*/
conrot_servo::conrot_servo()
	:present_velo(0.0), present_disp(0.0), t_max(0.0)
{
	//initially the point should be at an all-stop
}


		
//conrot_servo()
/*
This function is the class copy constructor.  It will create an exact
copy of it's given class instance.  All dynamic memory will be copied 
as well (so be careful, you could have a huge performance hit).
INPUT:	conrot_servo&	|Input class to copy	
OUTPUT:	none
*/
conrot_servo::conrot_servo(const conrot_servo& given)
	:present_velo(given.present_velo), present_disp(given.present_disp), t_max(given.t_max)
{

}



//disp()
/*
This function will produce the displacement with the given parameters.
This function is specific to the radius of the spools that I use,
and the futaba servos that I am using. 
INPUT:	float	|t_i  the initial time of the movement
	float 	|t_f  the ending time of the movement
	int 	|pw_i the initial pulse width of the motion
	int	|pw_f the final pulse width of the motion
	float	|init_disp the initial displacement of the point
	float	|t  the time to perform the calculation on
OUTPUT:	float	|displacement 
*/
double conrot_servo::disp(double t_i, double t_f, int pw_i, int pw_f, double init_disp, double t)
{
	double pw_diff = pw_f - pw_i;
	double t_diff = t_f - t_i;
	double pw_t_ratio = pw_diff/t_diff;

	double final = (Pi_R1200*pw_t_ratio*(t*t))+(((Pi_R600*pw_i)-five_halfs_Pi_R)*t)+init_disp;
	
	return final;
}



//velo()
/*
This function will produce the velocity from the gien parameters.
This function is specific to the radius of the spools that I use and
the futaba servos that I am using.
INPUT:	float	|t_i  the initial time of the movement
	float 	|t_f  the ending time of the movement
	int 	|pw_i the initial pulse width of the motion
	int	|pw_f the final pulse width of the motion
	float	|t  the time to perform the calculation for
OUTPUT:	float	|the velocity of the point
*/
double conrot_servo::velo(double t_i, double t_f, int pw_i, int pw_f, double t)
{
	double pw_diff = pw_f - pw_i;
	double t_diff = t_f - t_i;
	double pw_t_ratio = pw_diff/t_diff;
	
	double final = (Pi_R600*((pw_t_ratio*t)+pw_i)) - five_halfs_Pi_R;
	return final;
}
		


//velo_at_t
/*
This function will find the velocity of the string at the given time.
This time is given as some time displacement from the first command
sent. (t=0)
INPUT:	float	|time of desired velocity
OUTPUT:	float	|resulting velocity
*/
float conrot_servo::velo_at_t(float desired_time)
{
	if(desired_time > t_max)	//desired time is greater than sim
	{
		cout<<"Desired time is greater than simulation."<<endl;
		return 0;
	}

	set_parser();		//set parser to beginning of list
	float sim_velo = 0;
	int pw_f = 1500;		//begin at a stop
	int pw_i = 1500;
	float t_f = 0;			//begin at t = 0
	float t_i = 0;

	while(t_f < desired_time)	//go to the right element
	{
		//check if within limits
		if((t_f + (get_time()/1000)) > desired_time)
		{
			break;
		}
		//obtain values from next command
		t_i = t_f;
		t_f += (get_time()/1000);
		pw_i = pw_f;
		pw_f = get_pw();


		//compute the displacement after each command
		sim_velo = velo(t_i, t_f, pw_i, pw_f, t_f);


		//increment element 
		traverse();	
	}
	//calculate displacement within command	
	sim_velo = velo(t_i, t_f, pw_i, pw_f, desired_time);


	return sim_velo;	
}



//disp_at_t
/*
This function will find the displacement of the string from it's origin
point (when t was 0).  It will return this displacement in centimeters.
INPUT:	float	|time of desired displacement in seconds
OUTPUT:	float	|resulting displacement
*/
float conrot_servo::disp_at_t(float desired_time)
{
	float max = find_t_max();
	
	if(desired_time > max)
	{
		cout<<"Desired time is out of simulation bounds."<<endl;		return 0;
	}
	
	float new_disp = 0;
	float previous_disp = 0;
	int pw_f = 1500;		//begin at a stop
	int pw_i = 1500;
	float t_f = 0;			//begin at t = 0
	float t_i = 0;
	set_parser();

	while(t_f < desired_time)	//go to the right element
	{
		//check if within limits
		if((t_f + (get_time()/1000)) > desired_time)
		{
			break;
		}
		//obtain values from next command
		t_i = t_f;
		t_f += (get_time()/1000);
		pw_i = pw_f;
		pw_f = get_pw();
		previous_disp = new_disp;


		//compute the displacement after each command
		new_disp = disp(t_i, t_f, pw_i, pw_f, previous_disp, t_f);


		//increment element number
		traverse();
	}

	//calculate displacement within time	
	new_disp = disp(t_i, t_f, pw_i, pw_f, previous_disp, desired_time);

	
	return new_disp;	
}



//print_disp
/*
This function will create a comma separated value newline delimited
file to be put into a graphing program.  This is useful for showing
displacements in time for some kind of UI.  The units are milliseconds
for the timestamp and centimeters of displacement.
INPUT:	int	|Time step for each value
	char*	|name of the file
OUTPUT:	void
*/
void conrot_servo::print_disp(float timestep, char* fname)
{
	FILE* fd = fopen(fname, "w");	//open the file descriptor
	int t_sweep = 0;	//beginning time value
	float new_disp = 0;
	float init_disp = 0;
	int pw_f = 1500;		//begin at a stop
	int pw_i = 1500;
	float i = 0;
	float t_f = 0;			//begin at t = 0
	float t_i = 0;
	float max = find_t_max()/1000;		//get the total time of the list
	float t_motion = 0;

	set_parser();		//reset the list parser
	

	
	while(t_sweep <= max)	//while we haven't gone to the end
	{
		pw_i = pw_f;
		pw_f = get_pw();
		t_i = t_f;
		t_f += (get_time()/1000);
		t_motion = t_f - t_i;

	
		//capture displacment in steps of given variable
		for(i = 0; i < t_motion; i += timestep)
		{
			new_disp = disp(t_i, t_f, pw_i, pw_f, init_disp, i);
			fprintf(fd,"%f,%f\n",(i+t_i),new_disp);
		}

		init_disp = new_disp;	//capture the initial disp of next command
		if(!traverse())	//traverse to next node
		{
			break;
		}
		t_sweep = t_f;
	}

	fclose(fd);	//close open file
	return;
}


	

	




//print_velo
/*
This function will create a comma separated value, newline delimited
file.  It is intended to be used with a program like MatLab or Excel.
Could be useful for making a graph and performing analysis on the 
specific motion.
INPUT:	int	|time step between each calculation
	char*	|name of the file
OUTPUT:	void
*/
void conrot_servo::print_velo(float timestep, char* fname)
{
	FILE* fd = fopen(fname, "w");	//open the file descriptor
	int t_sweep = 0;	//beginning time value
	float new_velo = 0;
	int pw_f = 1500;		//begin at a stop
	int pw_i = 1500;
	float i = 0;
	float t_f = 0;			//begin at t = 0
	float t_i = 0;
	int max = find_t_max()/1000;		//get the total time of the list
	float t_motion = 0;
	set_parser();		//reset the list parser

	
	while(t_sweep <= max)	//while we haven't gone to the end
	{
		pw_i = pw_f;
		pw_f = get_pw();
		t_i = t_f;
		t_f += (get_time()/1000);
		t_motion = t_f - t_i;
		
		//capture velocity in steps of given variable
		for(i = 0; i <= t_motion; i += timestep)
		{
			new_velo = velo(t_i, t_f, pw_i, pw_f, i);
			fprintf(fd,"%f,%f\n",(i+t_i),new_velo);
		}
	
		if(!traverse())	//traverse to the next item
		{
			break;
		}
		
		t_sweep = t_f;
	}

	fclose(fd);
	return;
}



//find_t_max()
/*
This function traverses the linear linked list to calculate the 
total time for the motion to take place.  This is useful after 
every genetic swap.
INPUT:	void
OUTPUT:	float	|new t_max value
*/
float conrot_servo::find_t_max()
{
	set_parser();
	int is_last = 1;
	float time = 0;
	while(is_last)
	{
		time += get_time();	
		is_last = traverse();	//returns 0 and kicks loop on last element in list
	}
	return time;
}
